/* 3D Navier-Stokes Solver */
#include "typen.h"
#include "matrix.h"
#include "navier.h"
#include <math.h>
#include <stdlib.h>

#define DDP(L,M,R,DA,i) (S->ddPstar[DA][0][i]*L+S->ddPstar[DA][1][i]*M+S->ddPstar[DA][2][i]*R)



 
double Navier::CalcRes() 
{
   double  sq;
   norm_res_rjr0_vj_tj=0.0;
   int index;
   if(List_Fluid_end_zero==0) return 0.0;
   vector<int>::iterator it = List_Fluid_zero->begin();
   for(int m=0; m<List_Fluid_end_zero; m++) 
   {
      index= *it;
      
         /*sq = DDP(P[i-1][j][k],P[index],P[i+1][j][k],0,i) +                            
            DDP(P[i][j-1][k],P[index],P[i][j+1][k],1,j) +                            
            DDP(P[i][j][k-1],P[index],P[i][j][k+1],2,k) - RHS[index];*/
      sq = MatVecMult(P,index,1,0.0);
      it++;
    }
   return (sqrt(norm_res_rjr0_vj_tj)/(List_Fluid_end_zero)); //S->delt);
}

// solve linear system "\Delta p=RHS" using SOR iteration in a 8-color scheme
// boundary values are set before each SOR iteration 
// iteration is terminated when 
//   - the residue is too small 
//   - a maximum value of iterations is exceeded
// if SSOR is defined, symmetric SOR iteration is used. 
//

void Navier::Poisson() 
{
   double    res;
   // Use BICGStab-Solver
   res=BiCGStab();
}

inline double  Navier::MatVecMult(double *A,int index, int RES, double vjr0_tjsj_factor)
{
   static int d123[6] = {1,-1,dj,-dj,dk,-dk};
   static double h2M1 = 1.0/h2;
   //static double h2M1 = 1.0/h;
   static double coef_center = -6 * h2M1;
   double result;   
   double PMIN = 0.0;
   /*
      result= S->ddPstar[0][2][i]*A[i+1][j][k] + S->ddPstar[0][0][i]*A[i-1][j][k] + 
      S->ddPstar[1][2][j]*A[i][j+1][k] + S->ddPstar[1][0][j]*A[i][j-1][k] +
      S->ddPstar[2][2][k]*A[i][j][k+1] + S->ddPstar[2][0][k]*A[i][j][k-1] +
      (S->ddPstar[0][1][i]+S->ddPstar[1][1][j]+S->ddPstar[2][1][k]) * A[index]; 
   */

   
      
      
      
      result = A[index] * coef_center;
      for(int it=0; it <=5; it++)
      {  
         int index1 = index + d123[it];
         if((flag[index1] >= AIR) && (flag[index1] < SLIP_AIR))
         {
            result += PMIN * h2M1;
         }
         else
         {
            if((flag[index1] >= FLUID) && (flag[index1] < INFLOW))
                result += A[index1]*h2M1;
            else
                result += A[index]*h2M1;
          }
	  
       }
       if(RES > 0) 
       { result=RHS[index]-result;}
       else
       {  norm_vjr0_tjsj += result*vjr0_tjsj_factor;}
       norm_res_rjr0_vj_tj +=(result*result);
       return result; 
}

double Navier::BiCGStab() 
{
   double a,b,w,res=1E9,norm_vj,norm_r0,norm_sj,norm_rj ;
   double rj1r0, rjr0;
   int index;
   int iterations=0;
   double CoeffPreCond = h2/6.0;
   //double CoeffPreCond = h/6.0;
   double CoeffPreCondBack = 1.0/ CoeffPreCond;
   
   restart:
   norm_res_rjr0_vj_tj=norm_r0=0.0;
   
   vector<int>::iterator it = List_Fluid_zero->begin();
   for(int m=0; m<List_Fluid_end_zero; m++) 
   {
      index= *it;
      //SetPLocalBorder(P,index);
      BiCG_r0[index]=BiCG_pj[index]=BiCG_rj[index]=MatVecMult(P,index,1,0);
      it++;   
   }
   it = List_Fluid_zero->begin();
   for(int m=0; m<List_Fluid_end_zero; m++) 
   {
      index= *it;
      P[index] *= CoeffPreCondBack;  //  PRECONDITIONBACK P  
      it++;   
   }
   rjr0 = norm_res_rjr0_vj_tj;      
   norm_r0=sqrt(rjr0);
    
   if((res>=S->eps) && (iterations<S->itermax))
   {	
      do
      {
         norm_vjr0_tjsj=0.0;
         norm_res_rjr0_vj_tj=0.0;
         norm_rj=0.0;
  
         // APpj      
         it = List_Fluid_zero->begin();
         for(int m=0; m<List_Fluid_end_zero; m++) 
         {
               index= *it;
               MatBuf[index] = CoeffPreCond * BiCG_pj[index];  //  PRECONDITION pj 
	       norm_rj += BiCG_rj[index] * BiCG_rj[index]; // NEU
	       it++;
	  }
	  norm_rj = sqrt(norm_rj);  // NEU   
          it = List_Fluid_zero->begin();
          for(int m=0; m<List_Fluid_end_zero; m++) 
          {
               index= *it;
               //SetPLocalBorder(MatBuf,index);
               BiCG_vj[index] = MatVecMult(MatBuf,index,0,BiCG_r0[index]);
	       it++;  	
          }
	    
         norm_vj = sqrt(norm_res_rjr0_vj_tj);         
         a=rjr0 / norm_vjr0_tjsj ;
	    
         if(fabs(norm_vjr0_tjsj) <= (1e-12*(norm_vj*norm_r0)))
         {
              it = List_Fluid_zero->begin();
              for(int m=0; m<List_Fluid_end_zero; m++) 
              {
                  index= *it;
                  P[index] *=CoeffPreCond;  //  PRECONDITION P		
		  it++;
	      }	
	      res=rjr0/(List_Fluid_end_zero);
	      //res=CalcRes();
	      iterations++;
              goto restart ;
         }
         if((fabs(a)*norm_vj/(norm_rj==0?1E-15:norm_rj))<=0.08)
         {
             it = List_Fluid_zero->begin();
             for(int m=0; m<List_Fluid_end_zero; m++) 
             {
                  index= *it;
                  P[index]*=CoeffPreCond;  //  PRECONDITION P
		  it++;
	     }
	    res=rjr0/(List_Fluid_end_zero); 
            //res=CalcRes();
	    iterations++;	
            goto restart;  // NEU
         }
	 norm_sj=0.0; 
	  // sj =rj -a*vj
         it = List_Fluid_zero->begin();
         for(int m=0; m<List_Fluid_end_zero; m++) 
         {
             index= *it;
             BiCG_sj[index] = BiCG_rj[index]-a*BiCG_vj[index] ; 
             norm_sj += BiCG_sj[index]*BiCG_sj[index];
	     it++;
	 }   
         norm_sj=sqrt(norm_sj);
	    
         if(norm_sj>S->eps)
         {
            // tj = Asj 
            it = List_Fluid_zero->begin();
            for(int m=0; m<List_Fluid_end_zero; m++) 
            {
                  index= *it;
                  MatBuf[index] = CoeffPreCond*BiCG_sj[index];  //  PRECONDITION sj
		  it++;
            }
	    norm_vjr0_tjsj=norm_res_rjr0_vj_tj=0.0;
	    rj1r0=0.0;
	    res= 0.0;
            it = List_Fluid_zero->begin();
            for(int m=0; m<List_Fluid_end_zero; m++) 
            {
                index= *it;
                //SetPLocalBorder(MatBuf,index);		    
                BiCG_tj[index]=MatVecMult(MatBuf,index,0,BiCG_sj[index]); 
		it++;	
            }
            w=norm_vjr0_tjsj/((fabs(norm_res_rjr0_vj_tj)< 1E-15)?1E-15:norm_res_rjr0_vj_tj);
	    	
            it = List_Fluid_zero->begin();
            for(int m=0; m<List_Fluid_end_zero; m++) 
            {
                  index= *it;
                  P[index] += a*BiCG_pj[index] + w*BiCG_sj[index];
            // rj+1 = sj -w*tj
                  BiCG_rj[index] = BiCG_sj[index]-w*BiCG_tj[index] ;	
            // < rj+1, r0 >  
                  rj1r0 += BiCG_rj[index]*BiCG_r0[index] ;
		  res += BiCG_rj[index]*BiCG_rj[index]; 
	          it++;
	     }
             b=a*rj1r0/(w*rjr0==0?1E-15:(w*rjr0));
		
            it = List_Fluid_zero->begin();
            for(int m=0; m<List_Fluid_end_zero; m++) 
            {
               index= *it;
               BiCG_pj[index] = BiCG_rj[index] + b*(BiCG_pj[index]-w*BiCG_vj[index]);
	       it++;
	    }
		
         }
         else
         {
	    rj1r0=0.0 ;
	    res = 0.0;
            it = List_Fluid_zero->begin();
            for(int m=0; m<List_Fluid_end_zero; m++) 
            {
                  index= *it;
                  P[index] += a*BiCG_pj[index];
                  BiCG_rj[index] = BiCG_sj[index];
                  rj1r0 += BiCG_rj[index]*BiCG_r0[index] ;
		  res += BiCG_rj[index]*BiCG_rj[index];
		  it++; 
            }
		
         }
	    
         // j=j+1 
         rjr0 = rj1r0 ;
	 
         if(List_Fluid_end_zero == 0) res = 0.0;
         //else res=sqrt(res/(List_Fluid_end_zero)) /*S->delt*/;
	 else res=sqrt(res/(List_Fluid_end_zero)) /*S->delt*/;   
         iterations++;
	    	    
      }while( (res>=S->eps) && (iterations<S->itermax) ); 
	
   } // End If
    
    
   it = List_Fluid_zero->begin();
   for(int m=0; m<List_Fluid_end_zero; m++) 
   {
       index= *it; 
       P[index]*=CoeffPreCond;    //  PRECONDITION P
       it++;
   }
   res=CalcRes(); 
   cout<<"res = "<<res<<"\n";
   alliterations+=iterations;
   if(iterations>=S->itermax) cout << "iterations bound reached \n";
   return res;
}


